﻿#ifndef SERVOMANAGER_H
#define SERVOMANAGER_H

#include <QObject>
#include <QQueue>
#include <QTimer>
#include <QImage>
#include "CGraph/CGraph.h"

#include "./ServoBase/kincoservo.h"
#include "./ServoBase/RmServo.h"
#include "CameraManager.h"
#include <unordered_map>

class XYMoveNode;
class ZMoveNode;
class GrabNode;
class YStepNode;
class RotateNode;
class CameraNode;

class ServoManager : public QObject
{
    Q_OBJECT
public:
    explicit ServoManager(QObject *parent = nullptr);

    /*----------------普通出入柜指令动作步骤------------------*/
    const static uint8_t Action_None = 0;   //无动作
    const static uint8_t Action_InStorage = 1;  //进货位
    const static uint8_t Action_OutStorage = 2; //出货位
    const static uint8_t Action_ClampArv = 3;   //夹
    const static uint8_t Action_LoosenArv = 4;  //松
    const static uint8_t Action_MoveXY2PortTarget = 5;  //XY移动至出货口
    const static uint8_t Action_MoveXY2StorgeTarget = 6;    //XY移动至货位
    const static uint8_t Action_MoveXY2Origin = 7;  //XY移动至原点
    const static uint8_t Action_RationStep = 8; //Z轴和旋转同时移动
    const static uint8_t Action_YRaise = 9; //Y抬升
    const static uint8_t Action_YFall = 10; //Y下降
    const static uint8_t Action_Rotate = 11;    //旋转
    /*-----------------------找原点步骤----------------------*/
    const static uint8_t Action_ZFindOrigin = 12;   //Z找原点
    const static uint8_t Action_RFindOrigin = 13;   //旋转R找原点
    const static uint8_t Action_XYFindOrigin = 14;  //XY找原点
    /*-----------------------根据二维码定位货架动作步骤定义----------------------*/
    const static uint8_t Action_XLocate = 15;   //X轴定位
    const static uint8_t Action_YLocate = 16;   //Y轴定位
    const static uint8_t Action_QRScaning = 17;    //QR扫描中
    const static uint8_t Action_QRScaned = 18;    //QR扫描中
    /*----------------电机位置状态定义------------------*/
    const static int8_t Location_Origin = 0;    //原点
    const static int8_t Location_Origining = 1; //回原点中
    const static int8_t Location_FaceA = 2;     //A面，取货口面
    const static int8_t Location_FaceAing = 3;
    const static int8_t Location_FaceB = 4;    //B面，取货口对面
    const static int8_t Location_FaceBing = 5;
    const static int8_t Location_Moving = 6;  //非特定位置移动中
    const static int8_t Location_MoveDone = 16;  //非特定位置移动中
    const static int8_t Location_NoneOrigin = 7; //非原点位置
    const static int8_t Location_Porting = 8;   //XY出货口目标位置移动中
    const static int8_t Location_Port = 9;      //XY到达出货口目标位置
    const static int8_t Location_Clamping = 10; //抓取中
    const static int8_t Location_Clamped = 11;  //抓取完成
    const static int8_t Location_Loosening = 12;
    const static int8_t Location_Loosened = 13;
    const static int8_t Location_ZIn = 14;
    const static int8_t Location_ZOut = 15;

    /*----------------固定动作位置定义------------------*/
    const static int8_t Axis_Origin_INC = 0;
    const static int32_t Rotate_FaceA_INC = -395000;
    const static int32_t Rotate_FaceB_INC = 380000;
    const static int32_t AxisZ_In = -2300000;
    const static int32_t AxisZ_Out = 0;
    const static int32_t AxisY_RiseFall_INC = 50000;
    const static int32_t AxisY_Layer_INC = 1496060;  //柜子一层高度固定
    constexpr static float AxisX_1mm_INC = 1968.5;  //X轴1mm所对应的脉冲
    constexpr static float AxisY_1mm_INC = 3937;  //X轴1mm所对应的脉冲
    /*----------------分步调试状态定义------------------*/
    const static int8_t Debug_StepStatus_CmdNone = 0;
    const static int8_t Debug_StepStatus_CmdOK = 1;
    const static int8_t Debug_StepStatus_CmdNext = 2;
    const static int8_t Debug_StepStatus_CmdDoing = 3;

    /*----------------根据二维码定位货架位置定义------ ------------*/
    const static uint8_t QRLocateType_None = 0;
    const static uint8_t QRLocateType_X = 1;
    const static uint8_t QRLocateType_Y = 2;
    const static uint8_t QRLocateError = 3;

    void GraphBuild();

    bool SetServoMNT_PreOper(uint8_t nodeid);
    void SetServoAllMNT_PreOper();

    bool SetServoMNT_Oper(uint8_t nodeid);
    void SetServoAllMNT_Oper();

    void SetMoveXY(int32_t axisX,int32_t axisY,uint8_t movetype);
    void SetMoveZ(uint8_t type);
    void SetMoveZbyValue(int32_t);
    void SetMoveRotate(uint8_t type);
    void SetMoveRotatebyValue(int32_t angle);
    void SetMoveClamp(uint8_t clamp);
    void SetYStep(uint8_t type);
    void SetAllStop();

    void SetServoInitLocation();
    void SetSeekOriginZ();
    void SetSeekOriginXY();
    void SetSeekOriginR();
    void SetStartAllSeekOrigin();

    void SendLocateResult();
signals:
    void sig_SendServoData(QJsonObject);
    void sig_StartQRLocateScan(quint8 type,quint8 locator);
    void sig_StartTargetArvQRScan();
    void sig_imgResultToServer(QImage);
public slots:
    void slot_InitDevice();
    void slot_recvPDOFrame(CanopenPDOFram frame);
    void slot_recvSDOFrame(CanopenSDOFrame frame);
    void slot_timerOut();
    void slot_axisXTimeOut();
    void slot_axisYTimeOut();
    void slot_axisZTimeOut();
    void slot_rotateTimeOut();
    void slot_OriginTimeOut();
    void slot_QRLocateTimeOut();
    void slot_SetServoParam(uint8_t nodeid,int32_t objectindex,int32_t value);
    void slot_ReadRwParams(uint8_t nodeID);
    void slot_MoveCmd(QJsonObject cmd);
    void slot_FindLocatorDistance(bool,int);
    void slot_FindTargetArvQR(bool,int);
    void slot_startInOutCabCmd();
    void slot_startQRLocateCmd();
    void slot_imgCmd();
private:
    std::unordered_map<int,KincoServo *> kincoMotorMap;
    CanOpenBus  *pMyCanBus {nullptr};
    KincoServo *pXmotor {nullptr};
    KincoServo *pYmotor {nullptr};
    KincoServo *pZmotor {nullptr};
    KincoServo *pRmotor {nullptr};
    RmServo *pRmServo = nullptr;
    CameraManager* pCameraManager {nullptr};

    QTimer *ptimer = nullptr;
    QTimer *ptimer_AxisX = nullptr;
    QTimer *ptimer_AxisY = nullptr;
    QTimer *ptimer_AxisZ = nullptr;
    QTimer *ptimer_Rotate = nullptr;
    QTimer *ptimer_SeekOrigin = nullptr;
    QTimer *ptimer_QRLocate = nullptr;

    uint8_t mRm_IOStatus = 0;   //夹爪电机IO状态

    uint8_t mServoNo[5] = {1,2,3,4,5};
    uint8_t mcnt = 1;
    uint8_t mcnt_ActionTime = 0;
    uint8_t mcnt_ActionType = 0;
    uint8_t mcnt_Rmaction = 0;  //夹爪电机动作计时，暂时不用查询状态反馈来判断动作是否完成
    int32_t mPreTargetY = 0;    //保存上一条指令的Y，用于抬升和下降
    uint8_t mcnt_QRLocateType = 0;
    uint8_t mcnt_QRLocateCnt = 0;
    bool mIsFindLocator = false;
    int mLocatorDistance = 0;
    int mLocatorPosition = 0;
    bool mIsFindTargetQR = false;
    int mTargetArvDistance = 0;

    CGraph::GPipelinePtr pPort2StorgeLine {nullptr};
    CGraph::GPipelinePtr pStorge2PortLine {nullptr};
    /*存档案
        XY跑至出货口->Z进->夹->Y抬升->Z出(->RB)->XY跑目标位置->Z进->Y下降->松->Z出*/
    XYMoveNode *pP2S_XY_Port_Node {nullptr};
    CameraNode *pP2S_Img_Port_Node {nullptr};
    ZMoveNode *pP2S_ZIn_Port_Node {nullptr};
    GrabNode *pP2S_Grab_Port_Node {nullptr};
    YStepNode *pP2S_YStep_Port_Node {nullptr};
    ZMoveNode *pP2S_ZOut_Port_Node {nullptr};
    RotateNode *pP2S_RB_Node {nullptr};
    XYMoveNode *pP2S_XY_Storge_Node {nullptr};
    CameraNode *pP2S_Img_Storge_Node {nullptr};
    ZMoveNode *pP2S_ZIn_Storge_Node {nullptr};
    YStepNode *pP2S_YStep_Storge_Node {nullptr};
    GrabNode *pP2S_Grab_Storge_Node {nullptr};
    ZMoveNode *pP2S_ZOut_Storge_Node {nullptr};
    RotateNode *pP2S_RA_Node {nullptr};

    /*取档案
        XY跑目标货位(->RB)->Z进->夹->Y抬升->Z出->XY跑至出货口->Z进->Y下降->松->Z出*/
    XYMoveNode *pS2P_XY_Port_Node {nullptr};
    CameraNode *pS2P_Img_Port_Node {nullptr};
    ZMoveNode *pS2P_ZIn_Port_Node {nullptr};
    GrabNode *pS2P_Grab_Port_Node {nullptr};
    YStepNode *pS2P_YStep_Port_Node {nullptr};
    ZMoveNode *pS2P_ZOut_Port_Node {nullptr};
    RotateNode *pS2P_RB_Node {nullptr};
    XYMoveNode *pS2P_XY_Storge_Node {nullptr};
    CameraNode *pS2P_Img_Storge_Node {nullptr};
    ZMoveNode *pS2P_ZIn_Storge_Node {nullptr};
    YStepNode *pS2P_YStep_Storge_Node {nullptr};
    GrabNode *pS2P_Grab_Storge_Node {nullptr};
    ZMoveNode *pS2P_ZOut_Storge_Node {nullptr};
    RotateNode *pS2P_RA_Node {nullptr};


    friend class XYMoveNode;
    friend class ZMoveNode;
    friend class GrabNode;
    friend class YStepNode;
    friend class RotateNode;
    friend class CameraNode;
};

class BaseNode : public CGraph::GNode {
public:
    BaseNode(ServoManager * ptr) : pServoManager(ptr){};
    ServoManager *pServoManager {nullptr};
};

enum XYMOVETYPE{
    type_Origin,
    type_Port,
    type_Storge
};

class XYMoveNode : public BaseNode {
public:
    XYMoveNode(uint8_t ttype,ServoManager * ptr):BaseNode(ptr), type(ttype){};

    void setXY(const int32_t& xx, const int32_t& yy){
        x = xx;
        y = yy;
    }
    CStatus run() override;
private:
    uint8_t type {XYMOVETYPE::type_Origin};
    int32_t x {0};
    int32_t y {0};
};

enum ZMOVETYPE{
    type_In,
    type_Out
};

class ZMoveNode : public BaseNode {
public:
    ZMoveNode(uint8_t ttype,ServoManager * ptr):BaseNode(ptr),type(ttype){};
    CStatus run() override;
private:
    uint8_t type {ZMOVETYPE::type_In};
};

enum GRABTYPE{
    type_Clampe,
    type_Lossen
};

class GrabNode : public BaseNode {
public:
    GrabNode(uint8_t ttype,ServoManager * ptr):BaseNode(ptr),type(ttype){};
    CStatus run() override;
private:
    uint8_t type {GRABTYPE::type_Clampe};
};

enum YSTEPTYPE{
    type_UP,
    type_DOWN
};

class YStepNode : public BaseNode {
public:
    YStepNode(uint8_t ttype,ServoManager * ptr):BaseNode(ptr),type(ttype){};
    CStatus run() override;
private:
    uint8_t type {YSTEPTYPE::type_UP};
};

enum ROTATETYPE{
    type_FaceA,
    type_FaceB
};

class RotateNode : public BaseNode {
public:
    RotateNode(uint8_t ttype,ServoManager * ptr):BaseNode(ptr),type(ttype){};
    void setEnable(bool bl){enable = bl;};
    CStatus run() override;
private:
    uint8_t type {ROTATETYPE::type_FaceA};
    bool enable {false};
};

class CameraNode : public BaseNode {
public:
    CameraNode(ServoManager * ptr):BaseNode(ptr){};
//    void setEnable(bool bl){enable = bl;};
    CStatus run() override;
private:
//    uint8_t type {ROTATETYPE::type_FaceA};
//    bool enable {false};
};

#endif // SERVOMANAGER_H
